/*
 * Copyright 2022 MindMotion Microelectronics Co., Ltd.
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "at_server.h"
#include "board_init.h"

/*
 * Macros.
 */

#define AT_CMD_BUF_SIZE     128u  /* at command buffer size. */

/*
 * Declerations.
 */

/* to query the parameter format and value range for AT+LED command. */
at_result_t at_cmd_servo_test(at_server_write_t write);

/* to return command parameters. */
at_result_t at_cmd_servo_query(at_server_write_t write);

/* to set user-specified parameters into the corresponding function. */
at_result_t at_cmd_servo_setup(at_server_write_t write, char * args);

/* to perform related operations. */
at_result_t at_cmd_servo_exec(at_server_write_t write);

/* at command. */
const at_cmd_t at_cmd_servo =
{
    .name = "AT+SERVO",
    .args_expr = "<value>",
    .test  = at_cmd_servo_test,  /* AT+SERVO=?    */
    .query = at_cmd_servo_query, /* AT+SERVO?     */
    .setup = at_cmd_servo_setup, /* AT+SERVO=1, 0 */
    .exec  = at_cmd_servo_exec   /* AT+SERVO      */
};

/*
 * Implementations.
 */
typedef struct
{
    TIM_Type * port;
    uint32_t channel;
    uint32_t lower;
    uint32_t upper;
    uint32_t value;
} servo_pwm_ctrl_obj_t;

#define APP_SERVO_PWM_NUM  6u
servo_pwm_ctrl_obj_t servo_pwm_ctrl_objs[APP_SERVO_PWM_NUM] =
{
    {BOARD_SERVO_PWM_012_PORT, BOARD_SERVO_PWM_0_CHN, 250, 1250, 750},
    {BOARD_SERVO_PWM_012_PORT, BOARD_SERVO_PWM_1_CHN, 250, 1250, 750},
    {BOARD_SERVO_PWM_012_PORT, BOARD_SERVO_PWM_2_CHN, 250, 1250, 750},
    {BOARD_SERVO_PWM_345_PORT, BOARD_SERVO_PWM_3_CHN, 250, 1250, 750},
    {BOARD_SERVO_PWM_345_PORT, BOARD_SERVO_PWM_4_CHN, 250, 1250, 750},
    {BOARD_SERVO_PWM_345_PORT, BOARD_SERVO_PWM_5_CHN, 250, 1250, 750},
};

#define APP_SERVO_PWM_CTRL_PERIOD 1000u    /* 1000 step in control. */
#define APP_SERVO_PWM_STEP_FREQ   500000u  /* 500kHz resolution. */
#define APP_SERVO_UPDATE_PERIOD   20000u   /* 20ms period scale, the num of steps. */


void app_servo_ctrl_init(void)
{
    /* Setup the counters for counting step. */
    TIM_Init_Type tim_init;
    tim_init.ClockFreqHz = BOARD_SERVO_PWM_012_FREQ;
    tim_init.StepFreqHz = APP_SERVO_PWM_STEP_FREQ;
    tim_init.Period = APP_SERVO_UPDATE_PERIOD - 1u; /* the counter would return to the base on next step. */
    tim_init.EnablePreloadPeriod = false; /* no need preload, load period value immediately. */
    tim_init.PeriodMode = TIM_PeriodMode_Continuous;
    tim_init.CountMode = TIM_CountMode_Increasing;
    TIM_Init(BOARD_SERVO_PWM_012_PORT, &tim_init);
    
    tim_init.ClockFreqHz = BOARD_SERVO_PWM_345_FREQ;
    TIM_Init(BOARD_SERVO_PWM_345_PORT, &tim_init);

    /* Setup the PWM output channel. */
    TIM_OutputCompareConf_Type tim_out_conf;
    tim_out_conf.ChannelValue = 0u;
    tim_out_conf.EnableFastOutput = false;
    tim_out_conf.EnablePreLoadChannelValue = false; /* disable preload, load channel value immediately. */
    tim_out_conf.RefOutMode = TIM_OutputCompareRefOut_FallingEdgeOnMatch;
    tim_out_conf.ClearRefOutOnExtTrigger = false;
    tim_out_conf.PinPolarity = TIM_PinPolarity_Rising;
    
    for (uint32_t i = 0u; i < APP_SERVO_PWM_NUM; i++)
    {
        tim_out_conf.ChannelValue = servo_pwm_ctrl_objs[i].value;
        TIM_EnableOutputCompare(servo_pwm_ctrl_objs[i].port, servo_pwm_ctrl_objs[i].channel, &tim_out_conf);
    }

    /* Start the output compare, only available for the TIM peripheral with this feature. */
    TIM_EnableOutputCompareSwitch(BOARD_SERVO_PWM_012_PORT, true);
    TIM_EnableOutputCompareSwitch(BOARD_SERVO_PWM_345_PORT, true);

    /* Start the counter. */
    TIM_Start(BOARD_SERVO_PWM_012_PORT);
    TIM_Start(BOARD_SERVO_PWM_345_PORT);
}









/*
 * Functions.
 */

/* when sending AT+SERVO=? command will call this function. to qurey the format and the range of paramter of the command. */
at_result_t at_cmd_servo_test(at_server_write_t write)
{
    char buf[AT_CMD_BUF_SIZE];
    sprintf(buf, "AT+SERVO=<value0>,<value1>,<value2>,<value3>,<value4>,<value5>\r\n");
    write(buf, strlen(buf));

    return AT_RESULT_OK;
}

/* when sending AT+SERVO? command will call this function. to query the current value of the command. */
at_result_t at_cmd_servo_query(at_server_write_t write)
{
    char buf[AT_CMD_BUF_SIZE];

    sprintf(buf, "+SERVO=%d,%d,%d,%d,%d,%d\r\n", 
                    servo_pwm_ctrl_objs[0].value,
                    servo_pwm_ctrl_objs[1].value,
                    servo_pwm_ctrl_objs[2].value,
                    servo_pwm_ctrl_objs[3].value,
                    servo_pwm_ctrl_objs[4].value,
                    servo_pwm_ctrl_objs[5].value);

    write(buf, strlen(buf));

    return AT_RESULT_OK;
}

/* when sending AT+LED=<value>, the value can be 0 or 1 or other, will call this function. */
at_result_t at_cmd_servo_setup(at_server_write_t write, char * args)
{
    char buf[AT_CMD_BUF_SIZE];
    uint32_t params[6]; /* the command parameter. */

    if (6 == at_req_parse_args(args, "=%d %d %d %d %d %d", &params[0], &params[1], &params[2], &params[3], &params[4], &params[5] ))
    {
        for (uint32_t i = 0u; i < 6u; i++)
        {
            if ( (servo_pwm_ctrl_objs[i].lower <= params[i]) && (params[i] <= servo_pwm_ctrl_objs[i].upper) )
            {
                servo_pwm_ctrl_objs[i].value = params[i];
                TIM_PutChannelValue(servo_pwm_ctrl_objs[i].port, servo_pwm_ctrl_objs[i].channel, params[i]);
            }
        }

        sprintf(buf, "OK\r\n");
    }

    else
    {
        sprintf(buf, "ERROR \r\n");
    }
    write(buf, strlen(buf));

    return AT_RESULT_OK;
}

/* when sending AT+LED command will call this function. to exectue led initialization. */
at_result_t at_cmd_servo_exec(at_server_write_t write)
{
    char buf[AT_CMD_BUF_SIZE];
#if 0
    /* LED initialize. */
    GPIO_Init_Type gpio_init;
    gpio_init.Pins  = BOARD_LED0_GPIO_PIN;
    gpio_init.PinMode  = GPIO_PinMode_Out_PushPull;
    gpio_init.Speed = GPIO_Speed_50MHz;
    GPIO_Init(BOARD_LED0_GPIO_PORT, &gpio_init);
    GPIO_WriteBit(BOARD_LED0_GPIO_PORT, BOARD_LED0_GPIO_PIN, 1u);
#endif
    app_servo_ctrl_init();
    sprintf(buf, "OK\r\n");
    write(buf, strlen(buf));

    return AT_RESULT_OK;
}
/* EOF. */
